//
//  WalkBot.m
//  PezMonkey
//
//  Created by Spence DiNicolantonio on 12/25/09.
//  Copyright 2009 Necrosoft. All rights reserved.
//

#import "WalkBot.h"

#define kWalkBotMotorRate 2
#define kWalkBotMotorCoefFast 2
#define kWalkBotMotorCoefNormal 1
#define kWalkBotMotorCoefSlow .5
#define kWalkBotGroup 1


@implementation WalkBot

@synthesize motor;
@synthesize staticBody;

- (NSString *)demoName {
	return @"Walk Bot";
}

- (void)start {
	// create space
	self.space = [PMSpace spaceWithGravity:PMVectMake(0, -1)];
	[space setSubSteps:5];
	[space setIterations:15];
	
	// initialize shared static body
	self.staticBody = [PMBody staticBody];
	
	PMShape *shape;
	PMConstraint *joint;
	
	// add screen border
	[self addScreenBoundary];
	
	// define chassis attributes
	PMFloat chassisWidth = 150.0;
	PMFloat chassisHeight = 10.0;
	PMFloat chassisMass = 1.0;
	
	// define chassis vertices
	NSUInteger vertCount = 4;
	PMVect verts[] = {
		PMVectMake(-chassisWidth/2,-chassisHeight/2),
		PMVectMake(-chassisWidth/2, chassisHeight/2),
		PMVectMake( chassisWidth/2, chassisHeight/2),
		PMVectMake( chassisWidth/2,-chassisHeight/2),
	};
	
	// calculate chassis moment
	PMFloat chassisMoment = PMMomentForPolygon(chassisMass, verts, vertCount, PMVectZero);
	
	// create chassis body
	PMBody *chassis = [PMBody bodyWithMass:chassisMass 
									moment:chassisMoment];
	[chassis setPosition:PMVectMake(200, -150)];
	[space addBody:chassis];
	
	// create chassis shape
	shape = [PMPolygonShape polygonShapeWithBody:chassis 
										vertices:verts 
									 vertCount:vertCount];
	[shape setFriction:1.0];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kWalkBotGroup];
	[space addShape:shape];

	
	// define crankshaft attributes
	PMFloat crankMass = 1.0;
	PMFloat crankRadius = 20.0;
	PMFloat crankMoment = PMMomentForCircle(crankMass, crankRadius, 0.0, PMVectZero);
	PMFloat crankSpeed = (2.0 * M_PI) / 3.0;
	PMFloat crankXOffset = chassisWidth/2 - crankRadius;
	PMFloat crankYOffset = crankRadius + 30.0;
	PMVect crankPosition;
	
	// create crankshaft bodys
	PMBody *crank1 = [PMBody bodyWithMass:crankMass moment:crankMoment];
	crankPosition = PMVectAdd(chassis.position, PMVectMake(crankXOffset, crankYOffset));
	[crank1 setPosition:crankPosition];
	[space addBody:crank1];
	
	PMBody *crank2 = [PMBody bodyWithMass:crankMass moment:crankMoment];
	crankPosition = PMVectAdd(chassis.position, PMVectMake(-crankXOffset, crankYOffset));
	[crank2 setPosition:crankPosition];
	[space addBody:crank2];
	
	// create crankshaft shapes
	shape = [PMCircleShape circleShapeWithBody:crank1 radius:crankRadius];
	[shape setFriction:1.0];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kWalkBotGroup];
	[space addShape:shape];
	
	shape = [PMCircleShape circleShapeWithBody:crank2 radius:crankRadius];
	[shape setFriction:1.0];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kWalkBotGroup];
	[space addShape:shape];
	
	// attach crankshafts to chassis
	joint = [PMPivotJoint pivotJointWithAnchor:PMVectMake(crankXOffset, crankYOffset) 
									   onBodyA:chassis 
										anchor:PMVectZero 
									   onBodyB:crank1];
	[space addConstraint:joint];
	
	joint = [PMPivotJoint pivotJointWithAnchor:PMVectMake(-crankXOffset, crankYOffset) 
									   onBodyA:chassis 
										anchor:PMVectZero 
									   onBodyB:crank2];
	[space addConstraint:joint];
	

	// attach motor and gear joint to chassis and crankshaft
	self.motor = [PMSimpleMotor simpleMotorWithBodyA:chassis 
											   bodyB:crank1
												rate:crankSpeed];
	[space addConstraint:motor];
	joint = [PMGearJoint gearJointWithBodyA:crank1 
									  bodyB:crank2
									  phase:0 
									  ratio:1];
	[space addConstraint:joint];
		
	// create legs
	[self addLegToChassis:chassis crank:crank1 crankOffset:crankRadius];
	[self addLegToChassis:chassis crank:crank1 crankOffset:-crankRadius];
	[self addLegToChassis:chassis crank:crank2 crankOffset:crankRadius];
	[self addLegToChassis:chassis crank:crank2 crankOffset:-crankRadius];
}

- (void)update:(NSTimeInterval)deltaT {
	// determine motor max speed coefficient
	PMFloat coef = kWalkBotMotorCoefNormal;
	if (upArrow^downArrow)
		coef = (upArrow)? kWalkBotMotorCoefFast : kWalkBotMotorCoefSlow;
	
	// determine motor rate
	PMFloat rate = 0;
	if (leftArrow^rightArrow)
		rate = (leftArrow)? kWalkBotMotorRate : -kWalkBotMotorRate;
	rate *= coef;
	
	// set motor rate
	[motor setRate:rate];
}

/**
 * Creates and segment shapes to demo space to make a boundary along the edges of
 * the screen.
 */
- (void)addScreenBoundary {
	// make sure static body has been initialized
	if (!staticBody)
		self.staticBody = [PMBody staticBody];
	
	// create temp shape
	PMShape *shape;
	
	// define boundary segment width 
	// (needs to be thick to avoid penetration at high velocities)
	PMFloat width = 30;
	
	PMFloat vertCount = 4;
	
	// create left screen boundary
	PMVect leftVerts[] = {
		PMVectMake(-320 - width, -240),
		PMVectMake(-320 - width, 240),
		PMVectMake(-320, 240),
		PMVectMake(-320, -240)
	};
	shape = [PMPolygonShape polygonShapeWithBody:staticBody 
										vertices:leftVerts 
									 vertCount:vertCount];
	[shape setElasticity:1.0];
	[shape setFriction:1.0];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addStaticShape:shape];
	
	// create right screen boundary
	PMVect rightVerts[] = {
		PMVectMake(320 + width, 240),
		PMVectMake(320 + width, -240),
		PMVectMake(320, -240),
		PMVectMake(320, 240),
	};
	shape = [PMPolygonShape polygonShapeWithBody:staticBody 
										vertices:rightVerts 
									 vertCount:vertCount];
	[shape setElasticity:1.0];
	[shape setFriction:1.0];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addStaticShape:shape];
	
	// create bottom screen boundary
	PMVect bottomVerts[] = {
		PMVectMake(-320, -240 - width),
		PMVectMake(-320, -240),
		PMVectMake(320, -240),
		PMVectMake(320, -240 - width),
	};
	shape = [PMPolygonShape polygonShapeWithBody:staticBody 
										vertices:bottomVerts 
									 vertCount:vertCount];
	[shape setElasticity:1.0];
	[shape setFriction:1.0];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addStaticShape:shape];
}

- (void)addLegToChassis:(PMBody *)chassis crank:(PMBody *)crank crankOffset:(PMFloat)crankOffset {
	// define leg attributes
	PMFloat mass = 1.0;
	PMFloat length = 80.0;
	PMFloat width = 10.0;
	PMVect start = PMVectMake(0.0, length/2);
	PMVect end = PMVectMake(0.0, -length/2);
	PMFloat moment = PMMomentForSegment(mass, start, end);
	PMVect position = PMVectAdd(crank.position, PMVectMake(0.0, -length/2 + crankOffset));
	
	// create leg body
	PMBody *leg = [PMBody bodyWithMass:mass 
								 moment:moment];
	[leg setPosition:position];
	[space addBody:leg];

	// create leg shape
	PMShape *shape = [PMSegmentShape segmentShapeWithBody:leg 
													start:start 
													  end:end
													width:width];
	[shape setCollisionGroup:kWalkBotGroup];
	[space addShape:shape];

	
	PMConstraint *joint;
	
	// attach leg to crank
	joint = [PMPivotJoint pivotJointWithAnchor:PMVectMake(0.0, crankOffset) 
									   onBodyA:crank 
										anchor:PMVectMake(0.0, length/2) 
									   onBodyB:leg];
	[space addConstraint:joint];
	
	// attach leg to chassis
	PMSegment groove = PMSegmentMake(PMVectMake(0.0, length/2), 
									 PMVectMake(0.0, -length/2));
	PMVect anchor = PMVectMake(crank.position.x - chassis.position.x, 0.0);
	joint = [PMGrooveJoint grooveJointWithGroove:groove 
										 onBodyA:leg 
										  anchor:anchor 
										 onBodyB:chassis];
	[space addConstraint:joint];
		
	
	// add foot shape
	shape = [PMCircleShape circleShapeWithBody:leg 
										radius:10 
										center:PMVectMake(0, -length/2)];
	[shape setFriction:0.6];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kWalkBotGroup];
	[space addShape:shape];
}


#pragma mark -
#pragma mark Dealloc

- (void)dealloc {
	[staticBody release];
	[motor release];
	[super dealloc];
}

@end
